
//Maximum possible input from Joystick:
#define MAXINPUT 1.0

//Input Variables
int joyZ = 0;
float inpRow = 0;
float inpPit = 0;
float inpYaw = 0;
float maxJoyPitRoll = 0;
float maxJoyYaw = 0;

int a,b,c,d;

unsigned long lastUpdate;

float readVal(int val){
   return ((val * 2.0 - 256.0) / 256.0)*maxJoyPitRoll;
}
float readVal2(int val){
   return ((val * 2.0 - 256.0) / 256.0)*maxJoyYaw;
}

void joyInput(){
  
  while( Serial.available() > 0 ){
    
    byte id = Serial.peek();
    
    if( id == 'a' ){
      if( Serial.available() < 6 )
        break;
        
      Serial.read();
      int zVal = Serial.read();
      int pVal = Serial.read();
      int yVal = Serial.read();
      int rVal = Serial.read();
      int checkSum = Serial.read();
      
      if( checkSum != ((zVal+pVal+yVal+rVal) & 0xFF) )
        continue;
        
      a = zVal;
      b = pVal;
      c = yVal;
      d = rVal;
        
      eStop = false;
      
      joyZ = zVal;
      inpRow = readVal(rVal);
      inpPit = readVal(pVal);
      inpYaw = readVal2(yVal);
      
      lastUpdate = millis();
    
    } else if ( id == 'b' ) {
      if( Serial.available() < 4 )
        break;
      
      Serial.read();
      int rollPitchRange = Serial.read();
      int yawRange = Serial.read();
      int checkSum = Serial.read();
      
      if( checkSum != 0xFF )
        continue;
        
        maxJoyPitRoll = ((float)rollPitchRange/256);
        maxJoyYaw = ((float)yawRange/256);
    
    } else if ( id == 'e' ){
       Serial.read(); //Throw away the 'e' of the buffer
       eStop = true;
    } else {
      //Garbage charcter, throw it away!
      Serial.read();      
    }
    
  }
  
  if( millis() - lastUpdate > 1500){
    eStop = true;
  }  
}


